﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MeshSharp.Core.Models;
using MeshSharp.Core.Utilities;


namespace MeshSharp.Core
{
    public enum HeuristicType
    {
        Manhattan
    }

    public class NavMesh
    {

        #region Fields

        private Triangle[] triangles { get; set; }
        private Vector3[] vertices { get; set; }
        private List<EdgeNode> edges { get; set; }
        #endregion

        #region Properties
        public float Padding { get; set; }
        public HeuristicType Heuristic { get; set; }
        #endregion

        #region Public Methods

        public NavMesh(Vector3[] verts, int[] indices)
        {
            vertices = verts;
            edges = new List<EdgeNode>();
            //lets take care of our triangles first
            triangles = new Triangle[indices.Length / 3];
            for (int i = 0; i < indices.Length / 3; i++)
            {
                triangles[i] = new Triangle(indices[i * 3],
                                            indices[i * 3 + 1],
                                            indices[i * 3 + 2]);
            }
            
            //edges next
            //Here's how we do it:
            //We'll use a dictionary to store the data temporarily
            //using the edge's Center as the key because this value is unique
            //I didnt use a hash table because the order of the vertices in the 
            //EdgeNode may be different

            Dictionary<Vector3, EdgeNode> edgesDic = new Dictionary<Vector3, EdgeNode>();
            int triCountId = 0;
            foreach (Triangle tri in triangles)
            {
                //each triangle has an id
                tri.Id = ++triCountId; //increment after assignment

                //each triangle has 3 edges
                tri.Edges = new int[3];
                for (int i = 0; i < 3; i++)
                {
                    //gonna use modulus to loop through each pair of vertices
                    int ind1 = tri.Indices[i];
                    int ind2 = tri.Indices[(i + 1) % 3];

                    //next we find the center
                    Vector3 center = (verts[ind1] + verts[ind2]) / 2.0f;

                    EdgeNode node = new EdgeNode(new int[] { ind1, ind2 }, center);
                    //if the edge isnt there then we add it);
                    if (!edgesDic.ContainsKey(center))
                    {
                        edgesDic.Add(center, node);
                        edges.Add(node);
                    }
                    //add that edge to the list of edges of the triangle
                    tri.Edges[i] = edgesDic.Keys.ToList().IndexOf(center);

                    //one last thing, refer each triangle to the edge
                    edgesDic[center].Triangles.Add(triangles.ToList().IndexOf(tri));
                    //if the edge is shared by more than one triangles then it's shared
                    if (edgesDic[center].Triangles.Count > 1)
                        edgesDic[center].IsShared = true;
                }
            }

            //Generate the node graph
            foreach (var edge in edges)
            {
                if (edge.IsShared)
                {
                    
                }
            }

        }
        /// <summary>
        /// Core path finding function that fully utilises the nav mesh. This includes:
        /// Finding straight line first or
        /// Finding shortest path using A* with selected Heuristics.
        /// Funneling the path.
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public List<Vector3> GetPath(Vector3 start, Vector3 end)
        {
            //Start off with adding the start node first obviously.
            List<Vector3> path = new List<Vector3> {start};

            int startTriangle = getTriangleWithPoint(start);
            //If the start and end is in the same triangle, return them both.
            //This is used instead of raycast to prevent finding the triangle that contains the end point.
            if (MathUtils.IsInTriangle(getVerticesOfTriangle(startTriangle), end))
            {
                path.Add(end);
                return path;
            }

            int endTriangle = getTriangleWithPoint(end);
            //Check if path can be accomplished in a straight line between joined triangles
            if (rayCast(start, startTriangle, end, endTriangle))
            {
                path.Add(end);
                return path;
            }

            //TODO: Setup Pathfinder.
            path = findPath(start, startTriangle, end, endTriangle);
            //Use the PathFinder class to traverse the shared nodes.
            //PathFinder.AStar(navMesh.SharedEdgeNodes, nodeEnd, MathUtils.Distance, manhattanHeuristic);

            //TODO: Funnel the path in the mesh.

            return path;
        }

        public List<Vector3> GetPath(Vector3 start, Vector3 end, float paddding)
        {
            Padding = paddding;
            return GetPath(start, end);
        }
        #endregion

        #region Heuristic methods

        private double manhattanHeuristic(Vector3 newNode, Vector3 end)
        {
            return (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
        }

    #endregion

        #region Path Find

        List<Vector3> findPath(Vector3 start, int startTriangle,Vector3 end, int endTriangle)
        {
            Path<EdgeNode> path = Astar(start, startTriangle, end, endTriangle);
            if (path == null)
            {
                return null;
            }
            else
            {
                List<Vector3> vectorPath = new List<Vector3>();
                do
                {
                    vectorPath.Add(path.LastStep.Center);
                    path = path.PreviousSteps;
                } while (path != null);
                return vectorPath;
            }
        }

         Path<EdgeNode> Astar(Vector3 start, int startTriangle,Vector3 end, int endTriangle)
         {

            #if DEBUG
             System.Diagnostics.Debug.WriteLine("Next run");
             #endif
             //Assign actual shared edge as a start node.
             Path<EdgeNode> tempStartNode = new Path<EdgeNode>(new EdgeNode(null, start));
             EdgeNode tempEndNode = new EdgeNode(null, end);

             var closed = new HashSet<EdgeNode>();
             var queue = new PriorityQueue<double, Path<EdgeNode>>();
             queue.Enqueue(0, tempStartNode);

             bool isStartNodeInitilized = false;
             while (!queue.IsEmpty)
             {
                 var path = queue.Dequeue();

                 if (path.LastStep.Triangles.Count > 0)
                 {
                     //Check if the last step has any of the shared triangles containing the end node then connect them.
                     if (path.LastStep.Triangles.First() == endTriangle || path.LastStep.Triangles.Last() == endTriangle)
                     {
                         double d1 = MathUtils.Distance(path.LastStep.Center, tempEndNode.Center);
                         path = path.AddStep(tempEndNode, d1);
                     }
                 }
                 //Check if the closed list contains the previous node
                 if (closed.Contains(path.LastStep))
                     continue;
                 if (path.LastStep.Equals(tempEndNode))
                     return path;
                 closed.Add(path.LastStep);

                 //Connect the start node to the actual edges of the triangle its located in
                 if (!isStartNodeInitilized)
                 {
                     //Looping each edge
                     for (int j = 0; j < 3; j++)
                     {
                         EdgeNode n = edges[triangles[startTriangle].Edges[j]];
                         if (n.IsShared)
                         {
                             double d = MathUtils.Distance(path.LastStep.Center, n.Center);
                             var newPath = path.AddStep(n, d);
                             //queue.Enqueue(newPath.TotalCost + estimate(n), newPath);
                             queue.Enqueue(newPath.TotalCost + 0, newPath);

                         }
                     }
                     isStartNodeInitilized = true;
                 }
                 else
                 {
                     //Loop the 2 shared triangles
                     for (int a = 0; a < 2; a++)
                     {
                         //Loop each edge in the triangle
                         for (int i = 0; i < 3; i++)
                         {
                             EdgeNode n = edges[triangles[path.LastStep.Triangles[a]].Edges[i]];
                             if (n.IsShared && n != path.LastStep)
                             {
#if DEBUG
                                 //Print the Id of the traversed triangle
                                 System.Diagnostics.Debug.WriteLine(triangles[path.LastStep.Triangles[a]].Id);
#endif
                                 double d = MathUtils.Distance(path.LastStep.Center, n.Center);
                                 var newPath = path.AddStep(n, d);
                                 queue.Enqueue(newPath.TotalCost + 0, newPath);
                             }
                             
                             
                         }
                     }
                 }
             }
             return null;
         }


        #endregion

        #region Trigonmetry

        /// <summary>
        /// Returns the index of the triangle that contains the point, returns -1 if not found
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        private int getTriangleWithPoint(Vector3 point)
        {
            for (int a = 0; a < triangles.Length; a++ )
            {
                if (MathUtils.IsInTriangle(getVerticesOfTriangle(a), point))
                {
                    return a;
                }
            }
            return -1;
        }

        private Vector3[] getVerticesOfTriangle(int index)
        {
            return new[]{
                                        vertices[triangles[index].Indices[0]],
                                        vertices[triangles[index].Indices[1]], 
                                        vertices[triangles[index].Indices[2]]
                                    };
        }

        /// <summary>
        /// Test if a straight line can be accomplised through connected triangles.
        /// </summary>
        /// <param name="start"></param>
        /// <param name="startTriIndex"></param>
        /// <param name="end"></param>
        /// <param name="endTriIndex"></param>
        /// <returns></returns>
        private bool rayCast(Vector3 start, int startTriIndex, Vector3 end, int endTriIndex)
        {
            //Triangle that is currently being checked.
            Triangle currentTri = triangles[startTriIndex];
            //Temporary store the index of all the traversed shared edge index in a hashSet. 
            //HashSet.Contains() is O(1) speed.
            HashSet<int> traversedEdges = new HashSet<int>();

            //TODO: there is a better way to do this than loop all triangles, but I forgot :(
            //TODO: It has something to do with a while or do loop
            for (int a = 0; a <= triangles.Length; a++)
            {
                if (currentTri.Id == triangles[endTriIndex].Id)
                {
                    return true;
                }
                foreach (var e in currentTri.Edges)
                {
                    if (edges[e].IsShared && !traversedEdges.Contains(e))
                    {
                        //Add to traveresedEdge so we do not search the same edge again
                        traversedEdges.Add(e);

                        //Check if the straight path intersects with the shared edge
                        //If they do, continue into next triangle
                        Vector3 intersect = MathUtils.Intersect(vertices[edges[e].Indices[0]], vertices[edges[e].Indices[1]],
                                            start, end);
                        //Check if the point falls on the edge.
                        if (MathUtils.PointIsOnLine(intersect, vertices[edges[e].Indices[0]], vertices[edges[e].Indices[1]])
                            && MathUtils.PointIsOnLine(intersect,start, end))
                        {
                            //Compare next triangle to current triangle then assign the other.
                            currentTri = triangles[edges[e].Triangles[0]].Id == currentTri.Id ? triangles[edges[e].Triangles[1]] : triangles[edges[e].Triangles[0]];
                            break;
                        }
                    }
                }
            }
            return false;
        }

        #endregion
    }
}
